﻿
#include "lane-func.h"

using namespace lanecalc;

/////////////////////////////////////////////////////////////////////////////////////////

void lanecalc::calLine(Double ox, Double oy, Double yaw, LaneMode modeFirstOrder, LaneMode modeSecondOrder, LaneMode modeThirdOrder, Double values[4]/* in-out */)
{
	// get raw value
	double lA, lB, lC, lD; // 3, 2, 1, 0-order

	if (modeThirdOrder == LM_Clothoid)
	{
		lA = values[3] / 6.0;
	}
	else
	{
		lA = values[3];
	}

	if (modeSecondOrder == LM_Clothoid)
	{
		lB = values[2] / 2.0;
	}
	else
	{
		lB = values[2];
	}

	if (modeFirstOrder == LM_Clothoid)
	{
		lC = math::tan(values[1] * math::RAD2DEG + yaw);
	}
	else
	{
		lC = math::tan(math::atan(values[1]) + yaw);
	}

	lD = values[0];

	// X, Y offset
	values[3] = lA;
	values[2] = -3 * lA * ox + lB;
	values[1] = 3 * lA * ox * ox - 2 * lB * ox + lC;
	values[0] = -1 * lA * ox * ox * ox + lB * ox * ox - lC * ox + lD + oy;
}

/////////////////////////////////////////////////////////////////////////////////////////

void lanecalc::trackLine(LaneSensorSample sample, Optional<LaneSensorSample> lastSample, UInt& lineIDCount)
{
	const Double MaxGap = 0.5; // meter

	if (!lastSample.isValid())
	{
		for (UInt i = 0; i < sample->lines.size(); i++)
		{
			sample->lines[i].id = ++lineIDCount;
		}
		return;
	}

	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		LaneLine& target = sample->lines[i];
		for (UInt j = 0; j < lastSample.refValue()->lines.size(); j++)
		{
			LaneLine& ref = lastSample.refValue()->lines[j];

			if (target.rawID.valid && ref.rawID.valid)
			{
				if (target.rawID.value == ref.rawID.value)
				{
					target.id = ref.id;
					break;
				}
			}
			else if (target.modelValid() && ref.modelValid())
			{
				if (math::abs(target.param[0] - ref.param[0]) < MaxGap)
				{
					target.id = ref.id;
					break;
				}
			}
		}
		if (target.id <= 0) target.id = ++lineIDCount;
	}
}

/////////////////////////////////////////////////////////////////////////////////////////

class LineForSort
{
public:
	UInt index;
	Double pos;
};

Array<UInt> lanecalc::sortLines(LaneSensorSample sample)
{
	ArrayX<LineForSort> buf;
	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		LaneLine& line = sample->lines[i];
		if (!line.modelValid()) continue;
		if (line.frontEnd.valid && line.frontEnd.value < 0) continue;
		if (line.rearEnd.valid && line.rearEnd.value > 10) continue;

		LineForSort lineForSort;
		lineForSort.index = i;
		lineForSort.pos = sample->lines[i].param[0];

		buf.append(lineForSort);
	}

	Array<LineForSort> bufArray = buf.toArray();
	bufArray.sortAs([](auto& l1, auto& l2){ return l1.pos > l2.pos; });

	Array<UInt> output(bufArray.size());
	for (UInt i = 0; i < bufArray.size(); i++)
	{
		output[i] = bufArray[i].index;
	}
	return output;
}

/////////////////////////////////////////////////////////////////////////////////////////

OptionalDouble calLaneWidthSub(LaneLine& left, LaneLine& right)
{
	const Double MinWidth = 2.0; // w_min-0.3
	const Double MaxWidth = 4.1; // w_max+0.35=(w_min-0.25)*2

	Double angle = math::atan((left.param[1] + right.param[1]) / 2.0);
	Double rawWidth = math::abs(left.param[0] - right.param[0]);

	if (rawWidth == 0) return OptionalDouble();
	else rawWidth *= math::cos(angle);

	if (rawWidth >= MinWidth && rawWidth <= MaxWidth) return rawWidth;
	else return OptionalDouble();
}

OptionalDouble lanecalc::calLaneWidth(LaneSensorSample sample, Array<UInt> sortedIndex)
{
	if (sortedIndex.isEmpty()) return OptionalDouble();

	Double widthSum = 0;
	UInt widthCount = 0;
	for (UInt i = 0; i < sortedIndex.size() - 1; i++)
	{
		LaneLine& left = sample->lines[sortedIndex[i]];
		LaneLine& right = sample->lines[sortedIndex[i + 1]];

		OptionalDouble rawWidth = calLaneWidthSub(left, right);
		if (rawWidth.valid)
		{
			widthSum += rawWidth.value;
			widthCount++;
		}
	}

	return widthCount == 0 ? OptionalDouble() : (widthSum / widthCount);
}

OptionalDouble lanecalc::calLaneHeading(LaneSensorSample sample)
{
	const Double MaxAngle = 45; // deg
	const Double AngleThresh = 3; // deg

	Double angleSum = 0;
	UInt angleCount = 0;
	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		if (!sample->lines[i].modelValid()) continue;
		Double angle = math::atan(sample->lines[i].param[1]);
		if (math::abs(angle) > MaxAngle) continue;
		angleSum += angle;
		angleCount++;
	}

	if (angleCount < 2) return OptionalDouble();

	Double angleAvg = angleSum / angleCount;

	angleSum = 0;
	angleCount = 0;
	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		if (!sample->lines[i].modelValid()) continue;
		Double angle = math::atan(sample->lines[i].param[1]);
		if (math::abs(angle - angleAvg) > AngleThresh) continue;
		angleSum += angle;
		angleCount++;
	}

	return angleCount < 2 ? OptionalDouble() : (angleSum / angleCount);
}

OptionalDouble lanecalc::calLaneCurvature(LaneSensorSample sample)
{
	const Double MaxCurv = 0.2; // 1/m
	const Double CurvThresh = 0.01; // 1/m

	Double curvSum = 0;
	UInt curvCount = 0;
	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		if (!sample->lines[i].modelValid()) continue;
		Double curv = sample->lines[i].param[2] * 2;
		if (math::abs(curv) > MaxCurv) continue;
		curvSum += curv;
		curvCount++;
	}

	if (curvCount < 2) return OptionalDouble();

	Double curvAvg = curvSum / curvCount;

	curvSum = 0;
	curvCount = 0;
	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		if (!sample->lines[i].modelValid()) continue;
		Double curv = sample->lines[i].param[2] * 2;
		if (math::abs(curv - curvAvg) > CurvThresh) continue;
		curvSum += curv;
		curvCount++;
	}

	return curvCount < 2 ? OptionalDouble() : (curvSum / curvCount);
}

/////////////////////////////////////////////////////////////////////////////////////////

OptionalDouble lanecalc::calLateralVelocity(LaneSensorSample sample)
{
	Double stlcSum = 0;
	UInt stlcCount = 0;
	for (UInt i = 0; i < sample->lines.size(); i++)
	{
		OptionalDouble stlc = sample->getSTLC(sample->lines[i]);
		if (!stlc.valid) continue;

		Bool isLeft = sample->lines[i].param[0] > 0;
		if (isLeft) stlcSum += stlc.value;
		else stlcSum -= stlc.value;
		stlcCount++;
	}

	return stlcCount == 0 ? OptionalDouble() : stlcSum / stlcCount;
}

OptionalDouble lanecalc::calCenterDeparture(LaneSensorSample sample, Array<UInt> sortedIndex)
{
	if (sortedIndex.isEmpty()) return OptionalDouble();

	for (UInt i = 0; i < sortedIndex.size() - 1; i++)
	{
		LaneLine& leftLine = sample->lines[sortedIndex[i]];
		LaneLine& rightLine = sample->lines[sortedIndex[i + 1]];

		Double left0 = leftLine.param[0];
		Double right0 = rightLine.param[0];
		if (left0 <= 0 || right0 >= 0) continue;

		OptionalDouble leftDTLC = sample->getDTLC(leftLine);
		OptionalDouble rightDTLC = sample->getDTLC(rightLine);
		if (leftDTLC.valid && rightDTLC.valid) return (rightDTLC.value - leftDTLC.value) * 0.5;
	}
	return OptionalDouble();
}

/////////////////////////////////////////////////////////////////////////////////////////

void lanecalc::getFeatureLine(LaneSensorSample sample, Array<UInt> sortedIndex, OptionalInt& firstLeft, OptionalInt& secondLeft, OptionalInt& firstRight, OptionalInt& secondRight)
{
	if (sortedIndex.isEmpty()) return;

	const Double MaxDTLC = 3.0; // m

	for (Int i = 0; i < (Int)sortedIndex.size(); i++)
	{
		LaneLine& line = sample->lines[sortedIndex[i]];
		if (line.param[0] >= 0) continue;

		OptionalDouble dtlc = sample->getDTLC(line);
		if (!dtlc.valid) break;
		if (dtlc.value > MaxDTLC) break;

		firstRight = sortedIndex[i];

		if (i + 1 >= (Int)sortedIndex.size()) break;
		
		LaneLine& nextLine = sample->lines[sortedIndex[i + 1]];
		if (!calLaneWidthSub(line, nextLine).valid) break;

		secondRight = sortedIndex[i + 1];
		break;
	}

	for (Int i = (Int)sortedIndex.size() - 1; i >= 0; i--)
	{
		LaneLine& line = sample->lines[sortedIndex[i]];
		if (line.param[0] <= 0) continue;

		OptionalDouble dtlc = sample->getDTLC(line);
		if (!dtlc.valid) break;
		if (dtlc.value > MaxDTLC) break;

		firstLeft = sortedIndex[i];

		if (i - 1 < 0) break;

		LaneLine& nextLine = sample->lines[sortedIndex[i - 1]];
		if (!calLaneWidthSub(nextLine, line).valid) break;

		secondLeft = sortedIndex[i - 1];
		break;
	}
}